#include <sys/ioctl.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string>
#include <limits>
#include <bot_core/bot_core.h>
#include <bot_param/param_client.h>

#include <lcm/lcm-cpp.hpp>
#include <ConciseArgs>
#include "asctec_ll_data_structures.h"
#include <lcmtypes/asctec_ll_driver.hpp>

#define ring_buff_SIZE 2<<12

namespace asctec_ll_driver {

struct Subscription {
  Subscription(float freq, unsigned short request_bit)
      : last_request_(bot_timestamp_now() - 1),
        request_bit_(request_bit) {
    request_interval = 1e6 * 1.0 / freq;
  }
  int64_t last_request_;
  int64_t request_interval;  //in microseconds
  unsigned short request_bit_;
};

class App {

 public:
  App(int argc, char ** argv);

  void ReadSerialData();
  void SendDataRequest();

  void PublishLLStatus(const asctec_ll_status_t * ll);
  void PublishRCData(const asctec_rc_rawdata_t * rc);

 private:
  lcm::LCM * lcm;

  std::string serial_device;
  int baud;
  int serial_fd;

  std::vector<Subscription> subscriptions;

  BotRingBuf * ring_buff;

  BotParam * param;

};

static gboolean serialReadHandler(GIOChannel *source, GIOCondition condition,
                                  gpointer user_data) {
  App * self = (App *) user_data;
  self->ReadSerialData();
  return TRUE;
}

static gboolean request_timer_handler(gpointer user) {
  App * self = (App *) user;
  self->SendDataRequest();
  return TRUE;
}

void App::SendDataRequest() {

  int64_t now = bot_timestamp_now();
  unsigned short request = 0;
  for (int i = 0; i < subscriptions.size(); i++) {
    if (now - subscriptions[i].last_request_
        > subscriptions[i].request_interval) {
      request |= subscriptions[i].request_bit_;
      // It's more consistent to increment by the interval
      subscriptions[i].last_request_ += subscriptions[i].request_interval;
    }
  }
  if (request == 0)
    return;

  const int start_string_sz = 4;
  const int request_packet_size = start_string_sz + sizeof(request);
  char request_packet[request_packet_size] = { '>', '*', '>', 'p' };
  memcpy(request_packet + start_string_sz, &request, sizeof(request));
  bot_fileutils_write_fully(serial_fd, request_packet, request_packet_size);
}

void App::PublishLLStatus(const asctec_ll_status_t * stat) {
  ll_status_t msg;

  double v1 = stat->battery_voltage_1 / 1000.0;
  double v2 = stat->battery_voltage_1 / 1000.0;

  msg.voltage = fmax(v1, v2);
  if (msg.voltage < 10) {
    printf("LOW BATTERY WARNING!!! BEEP! \a \n");
    printf("LOW BATTERY WARNING!!! BEEP! \a \n");
    printf("LOW BATTERY WARNING!!! BEEP! \a \n");
    printf("LOW BATTERY WARNING!!! BEEP! \a \n");
  }

  msg.up_time = stat->up_time;
  msg.flightMode = stat->flightMode;
  msg.motors_spinning = stat->flying;

  msg.utime = bot_timestamp_now();

  lcm->publish("ASCTEC_LL_STATUS", &msg);
  fprintf(stderr, ".");
}

void App::PublishRCData(const asctec_rc_rawdata_t * rc) {
  rc_data_t msg;
  msg.lock = rc->lock;
  for (int i = 0; i < 8; i++) {
    msg.calib_values[i] = rc->channels_out[i];
    msg.raw_values[i] = rc->channels_in[i];
  }
  lcm->publish("ASCTEC_LL_RC_DATA", &msg);
}

void App::ReadSerialData() {
  int new_bytes = bot_ringbuf_fill_from_fd(ring_buff, serial_fd, -1);

  int num_bytes_read = 0;
  while (bot_ringbuf_available(ring_buff) > min_packet_size) {

    unsigned char packet_descriptor;
    int16_t packet_payload_size;
    unsigned short computed_crc;
    int header_size = asctec_packet_startstring_size
        + sizeof(packet_payload_size) + sizeof(packet_descriptor);

    const uint8_t* buf;
    // find the packet start

    bool found_packet_start = false;
    while (bot_ringbuf_available(ring_buff) > header_size) {
      buf = bot_ringbuf_peek_buf(ring_buff, header_size);
      if (memcmp(asctec_packet_startstring, buf, asctec_packet_startstring_size)
          == 0) {
        found_packet_start = true;
        break;
      }
      //header not here... deque 1 byte from ring_buff
      num_bytes_read++;
      bot_ringbuf_flush(ring_buff, 1);
    }

    if (!found_packet_start)
      break;

    // === read & parse packet data ===
    memcpy(&packet_payload_size, buf + asctec_packet_startstring_size,
           sizeof(packet_payload_size));

    if (packet_payload_size > max_packet_size) {
      fprintf(
          stderr,
          "ERROR!!! PACKET SIZE  of %d TOO BIG! Discarding it, there are %d bytes in buffer \n",
          packet_payload_size, bot_ringbuf_available(ring_buff));
      bot_ringbuf_flush(ring_buff, 1);
      continue;
    }

    int total_packet_size = header_size + packet_payload_size
        + sizeof(computed_crc) + sizeof(asctec_packet_stopstring);
    if (packet_payload_size <= 0) {
      // corrupt/invalid packet
      fprintf(
          stderr,
          "ERROR packet_size = %hd, and there are %d bytes in the buffer, discarding this packet\n",
          packet_payload_size, bot_ringbuf_available(ring_buff));
      bot_ringbuf_flush(ring_buff, 1);
      continue;
    } else if (bot_ringbuf_available(ring_buff) < total_packet_size) {
      //Not enough bytes in the buffer
      break;
    }

    buf = bot_ringbuf_peek_buf(ring_buff, total_packet_size);

    //skip past asctec_packet_start and packet_size to packet descriptor:
    const uint8_t *packet_discriptor_p = buf + asctec_packet_startstring_size
        + sizeof(packet_payload_size);

    packet_descriptor = *packet_discriptor_p;

    //skip past packet_descriptor to the actual packet
    const uint8_t *packet_payload_p = buf + header_size;

    //check crc
    computed_crc = asctec_crc16(packet_payload_p, packet_payload_size);
    // read checksum
    unsigned short received_crc16;
    memcpy(&received_crc16, packet_payload_p + packet_payload_size,
           sizeof(received_crc16));
    if (received_crc16 != computed_crc) {
      //bot_tictoc("corrupt");
      fprintf(stderr, "bad checksum on packet!\n");
      bot_ringbuf_flush(ring_buff, 1);
      continue;
    }

    //check the packet trailer
    if (memcmp(asctec_packet_stopstring,
               buf + total_packet_size - asctec_packet_stopstring_size,
               asctec_packet_stopstring_size) == 0) {
      fprintf(stderr, "warning no packet trailier!\n");
      bot_ringbuf_flush(ring_buff, 1);
      continue;
    }

    switch (packet_descriptor) {
      case ASCTEC_PD_LLSTATUS:  // 0x02
      {
        if (packet_payload_size == sizeof(asctec_ll_status_t)) {
          const asctec_ll_status_t * ll =
              (asctec_ll_status_t *) packet_payload_p;
          PublishLLStatus(ll);

        } else {
          fprintf(stderr, "\t--> INVALID packet size: %hd, should be %zu\n",
                  packet_payload_size, sizeof(asctec_ll_status_t));
        }
        break;
      }
      case ASCTEC_PD_RCDATA: {
        if (packet_payload_size == sizeof(asctec_rc_rawdata_t)) {
          const asctec_rc_rawdata_t * rc =
              (asctec_rc_rawdata_t *) packet_payload_p;
          PublishRCData(rc);
        } else {
          fprintf(stderr, "\t--> INVALID packet size: %hd, should be %zu\n",
                  packet_payload_size, sizeof(asctec_rc_rawdata_t));
        }
        break;
      }
      default: {
        fprintf(stderr, "\n* Unknown Packet %hd bytes *\n",
                packet_payload_size);
        break;
      }
    }  //close switch

    bot_ringbuf_flush(ring_buff, total_packet_size);

  }

  return;
}

App::App(int argc, char ** argv) {
  serial_device = "/dev/ttyUSB0";
  baud = 57600;

  float ll_status_freq = 2;
  float rc_data_freq = 20;

  ConciseArgs args(argc, argv);
  args.add(serial_device, "d", "device", "Serial device to open");
  args.add(ll_status_freq, "s", "status_freq",
           "Frequency to request ll_status");
  args.add(rc_data_freq, "r", "rc_data_freq", "Frequency to request rc_status");
  args.parse();

  serial_fd = bot_serial_open(serial_device.c_str(), baud, 1);
  lcm = new lcm::LCM();
  ring_buff = bot_ringbuf_create(ring_buff_SIZE);

  fprintf(stderr, "Opening %s at %d baud\n", serial_device.c_str(), baud);
  fprintf(stderr, "Polling ll_status at %f.1Hz\n", ll_status_freq);
  fprintf(stderr, "Polling rc_data at %f.1Hz\n", rc_data_freq);

  subscriptions.push_back(
      Subscription(ll_status_freq, ASCTEC_REQUEST_LL_STATUS_BIT));
  subscriptions.push_back(
      Subscription(rc_data_freq, ASCTEC_REQUEST_RC_DATA_BIT));

// set timer to send requests
  g_timeout_add(1000 * .01, request_timer_handler, this);

  bot_glib_mainloop_attach_lcm(lcm->getUnderlyingLCM());

  GIOChannel * ioc = g_io_channel_unix_new(serial_fd);

  g_io_add_watch_full(ioc, G_PRIORITY_HIGH, G_IO_IN,
                      (GIOFunc) serialReadHandler, this, NULL);

}

}  // namespace asctec_sdk_driver

int main(int argc, char ** argv) {

  GMainLoop * mainloop = g_main_loop_new(NULL, FALSE);

  asctec_ll_driver::App self(argc, argv);

  bot_signal_pipe_glib_quit_on_kill(mainloop);
  g_main_loop_run(mainloop);

  return 0;
}
